﻿#include "IonosphericCorr.h"
#include "literal.h"
#include <iomanip>

// 默认构造函数：使用默认Klobuchar参数
IonosphericCorr::IonosphericCorr()
{
    // 初始化默认Klobuchar参数
    for (int i = 0; i < 4; i++)
    {
        alphaParams[i] = 0.0;
        betaParams[i] = 0.0;
    }

    // 创建WGS84坐标系对象
    GPSConstant gpsConst;
    coordSys = new WGS84(gpsConst.get_refEllip());
}

// 带参数构造函数：设置Klobuchar参数
IonosphericCorr::IonosphericCorr(double alpha[4], double beta[4])
{
    // 复制Klobuchar参数
    for (int i = 0; i < 4; i++)
    {
        alphaParams[i] = alpha[i];
        betaParams[i] = beta[i];
    }

    // 创建WGS84坐标系对象
    GPSConstant gpsConst;
    coordSys = new WGS84(gpsConst.get_refEllip());
}

// 析构函数：释放动态分配的内存
IonosphericCorr::~IonosphericCorr()
{
    if (coordSys != nullptr)
    {
        delete coordSys;
        coordSys = nullptr;
    }
}

// 设置Klobuchar模型参数
void IonosphericCorr::set_klb_params(double alpha[4], double beta[4])
{
    // 复制α参数
    for (int i = 0; i < 4; i++)
    {
        alphaParams[i] = alpha[i];
    }

    // 复制β参数
    for (int i = 0; i < 4; i++)
    {
        betaParams[i] = beta[i];
    }
}

// Klobuchar模型电离层延迟改正（重载1：使用坐标和时间）
double IonosphericCorr::klobuchar_corr(const XYZ& receiverPos, const XYZ& satPos,
    BaseT obsTime, double frequency)
{
    double elevation, azimuth;

    // 计算高度角和方位角
    compute_elevation_azimuth(receiverPos, satPos, elevation, azimuth);

    // 转换接收机坐标为BLH
    BLH receiverBLH = coordSys->XYZ2BLH(receiverPos);

    // 调用重载函数
    return klobuchar_corr(elevation, azimuth, receiverBLH, obsTime);
}

// Klobuchar模型电离层延迟改正（重载2：使用角度和BLH坐标）
double IonosphericCorr::klobuchar_corr(double elevation, double azimuth,
    const BLH& receiverBLH, BaseT obsTime)
{


    // 将高度角转换为弧度
    double elevRad = elevation * D2R;

    // 计算倾斜因子
    double slantFactor = 1.0 + (16.0 * pow((0.53 - elevation / 90.0), 3));

    // 计算电离层穿刺点的地心角
    double psi = compute_earth_central_angle(receiverBLH, elevation, azimuth);

    // 计算电离层穿刺点的纬度
    double phiI = receiverBLH.B * D2R + psi * cos(azimuth * D2R);
    if (phiI > 0.416)
    {
        phiI = 0.416;
    }
    else if (phiI < -0.416)
    {
        phiI = -0.416;
    }

    // 计算电离层穿刺点的经度
    double lambdaI = receiverBLH.L * D2R +
        (psi * sin(azimuth * D2R)) / cos(phiI);

    // 计算地磁纬度
    double phiM = phiI + 0.064 * cos((lambdaI - 1.617) * D2R);

    // 计算当地时间
    double t = 43200.0 * lambdaI / PI + obsTime.sec % 86400;
    if (t >= 86400.0)
    {
        t -= 86400.0;
    }
    else if (t < 0.0)
    {
        t += 86400.0;
    }

    // 计算振幅
    double AMP = alphaParams[0] + alphaParams[1] * phiM +
        alphaParams[2] * phiM * phiM + alphaParams[3] * phiM * phiM * phiM;
    if (AMP < 0.0)
    {
        AMP = 0.0;
    }

    // 计算周期
    double PER = betaParams[0] + betaParams[1] * phiM +
        betaParams[2] * phiM * phiM + betaParams[3] * phiM * phiM * phiM;
    if (PER < 72000.0)
    {
        PER = 72000.0;
    }

    // 计算相位
    double X = 2.0 * PI * (t - 50400.0) / PER;

    // 计算电离层延迟
    double F = slantFactor;
    double ionoDelay;

    if (fabs(X) < 1.57)
    {
        ionoDelay = F * (5.0e-9 + AMP * (1.0 - X * X / 2.0 + X * X * X * X / 24.0));
    }
    else
    {
        ionoDelay = F * 5.0e-9;
    }

    // 转换为距离误差（米）
    const double SPEED_OF_LIGHT = 299792458.0;
    return ionoDelay * SPEED_OF_LIGHT;
}

// 计算高度角和方位角
void IonosphericCorr::compute_elevation_azimuth(const XYZ& receiverPos,
    const XYZ& satPos,
    double& elevation,
    double& azimuth)
{
    // 计算ENU坐标
    ENU enuPos = coordSys->XYZ2ENU(receiverPos, satPos);

    // 计算高度角（度）
    double range = sqrt(enuPos.e * enuPos.e + enuPos.n * enuPos.n + enuPos.u * enuPos.u);
    elevation = asin(enuPos.u / range) / D2R;

    // 计算方位角（度）
    azimuth = atan2(enuPos.e, enuPos.n) / D2R;
    if (azimuth < 0.0)
    {
        azimuth += 360.0;
    }
}

// 计算地心角
double IonosphericCorr::compute_earth_central_angle(const BLH& receiverBLH,
    double elevation,
    double azimuth)
{
    const double EARTH_RADIUS = 6371000.0;     // 地球半径（米）
    const double IONO_HEIGHT = 350000.0;       // 电离层高度（米）

    double elevRad = elevation * D2R;
    double RE = EARTH_RADIUS;
    double hI = IONO_HEIGHT;

    // 计算地心角
    double psi = 0.0137 / (elevRad / D2R + 0.11) - 0.022;

    return psi;
}

// 计算地磁纬度
double IonosphericCorr::compute_geomagnetic_latitude(double latitude,
    double longitude)
{
    // 简化的地磁纬度计算
    return latitude * D2R;
}

// 测试函数：验证电离层延迟改正功能
void IonosphericCorr::test(const XYZ& receiverPos, const XYZ& satPos, BaseT obsTime)
{
    cout << "/* 电离层延迟误差改正测试 */" << endl;

    // 计算高度角和方位角
    double elevation, azimuth;
    compute_elevation_azimuth(receiverPos, satPos, elevation, azimuth);

    cout << "卫星高度角: " << fixed << setprecision(2) << elevation << " 度" << endl;
    cout << "卫星方位角: " << fixed << setprecision(2) << azimuth << " 度" << endl;

    // 计算电离层延迟改正
    double ionoCorr = klobuchar_corr(receiverPos, satPos, obsTime, 1575.42e6);

    cout << "Klobuchar模型电离层延迟改正: " << scientific << setprecision(4)
        << ionoCorr << " 米" << endl;
    cout << endl;
}